#ifndef _CAR_H_
#define _CAR_H_

#include "pins.h"
#include <Servo.h>

#define _dir_forward     1
#define _dir_stopped     0
#define _dir_backward   -1

class Car {
private:
    Servo servo;
    const int DEFAULT_ANGLE = 90;
    const int LEFT          = DEFAULT_ANGLE - 30;
    const int RIGHT         = DEFAULT_ANGLE + 30;
    int angle      = DEFAULT_ANGLE;
    int dir        = _dir_stopped;
    int backup_dir = dir;

    void turn_to (int to);
public :
    Car ();
    void forward ();
    void pause ();
    void backward ();
    void turnLeft ();
    void turnRight ();
    void resetDir ();
};

Car::Car () {
    pinMode (PIN_MOTOR_0, OUTPUT);
    pinMode (PIN_MOTOR_1, OUTPUT);
    pinMode (PIN_EN, OUTPUT);
    servo.attach (PIN_DIRECTION);
    servo.write (DEFAULT_ANGLE);

    analogWrite (PIN_EN, 900);
}

void Car::forward () {
    if (dir == _dir_backward) {
        pause ();
        delay (20);
    }
    digitalWrite (PIN_MOTOR_0, LOW);
    digitalWrite (PIN_MOTOR_1, HIGH);
    dir = _dir_forward;
}

void Car::backward () {
    if (dir == _dir_forward) {
        pause ();
        delay (20);
    }
    digitalWrite (PIN_MOTOR_0, HIGH);
    digitalWrite (PIN_MOTOR_1, LOW);
    dir = _dir_backward;
}

void Car::pause () {
    digitalWrite (PIN_MOTOR_0, LOW);
    digitalWrite (PIN_MOTOR_1, LOW);
    backup_dir = dir;
    dir = _dir_stopped;
}

void Car::turnLeft () {
    digitalWrite (PIN_LEFT_LED, HIGH);
    digitalWrite (PIN_RIGHT_LED, LOW);
    turn_to (LEFT);
}

void Car::turnRight () {
    digitalWrite (PIN_LEFT_LED, LOW);
    digitalWrite (PIN_RIGHT_LED, HIGH);
    turn_to (RIGHT);
}

void Car::resetDir () {
    digitalWrite (PIN_LEFT_LED, LOW);
    digitalWrite (PIN_RIGHT_LED, LOW);
    turn_to (DEFAULT_ANGLE);
}

void Car::turn_to (int to) {
//    pause ();
    servo.write (to);
    delay (250);
/*
    if (backup_dir == _dir_forward) {
        forward ();
    } else if (backup_dir == _dir_backward) {
        backward ();
    }
*/
}

#endif
